
package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 * Implements line following for the markings on the field.
 * @author SuperNURDs
 */
public class LineTracker {
    private RobotDrive drive; // robot drive base object
    private DigitalInput left; // digital inputs for line tracking sensors
    private DigitalInput middle;
    private DigitalInput right;
    private DriverStation ds; // driver station object for getting selections
    private double defaultSteeringGain; // the default value for the steering gain

    // the power profiles for the straight and forked robot path. They are
    // different to let the robot drive more slowly as the robot approaches
    // the fork on the forked line case.
    public double forkProfile[] = {0.80, 0.8, 0.8, 0.7, 0.6, 0.5, 0.5, 0.45};
    public double straightProfile[] = {0.80, 0.8, 0.8, 0.7, 0.6, 0.5, 0.5, 0.45};
    double powerProfile[];   // the selected power profile\

    boolean straightLine; // whether we are following a straight line
    boolean goLeft;       // whether we are following a left branch
    double stopTime;      // when to start looking for end
    double time;          // current time
    int binaryValue;      // a single binary value of the three line tracking
                          // sensors

    int previousValue = 0; // the binary value from the previous loop
    Timer timer;
    boolean atCross = false; // if robot has arrived at end

    /**
     * Constructs a new LineTracker class.
     * @param _drive the RobotDrive object to use for driving.
     * @param leftInput left photosensor input.
     * @param middleInput middle photosensor input.
     * @param rightInput right photosensor input.
     * @param gain turn gain - how strongly the controller reacts.
     */
    protected LineTracker(RobotDrive drive, int leftInput, int middleInput, int rightInput, float gain) {
        // assign the robot drive
        this.drive = drive;

        // create the digital input objects to read from the sensors
        left = new DigitalInput(leftInput);
        middle = new DigitalInput(middleInput);
        right = new DigitalInput(rightInput);

        // get the driver station instance to read the digital I/O pins
        ds = DriverStation.getInstance();

        defaultSteeringGain = gain;
    }

    public LineTracker(RobotDrive drive){
        this(drive, 11, 12, 13, -0.5f);
    }

    public void reset(){
        // set the straightLine and left-right variables depending on chosen path
        straightLine = ds.getDigitalIn(1);
        goLeft = !ds.getDigitalIn(2) && !straightLine;
        // set appropriate power profile
        powerProfile = (straightLine) ? straightProfile : forkProfile;
        // set when the robot should look for end
        stopTime = (straightLine) ? 2.0 : 4.0;
        // print path choice
        System.out.println("Resetting Line Tracker");
        System.out.println("StraightLine: " + straightLine);
        System.out.println("GoingLeft: " + goLeft);

        time = 0; // reset current time
        atCross = false; // reset end condition variable

        // time the path over the line
        timer = new Timer();
        timer.start();
        timer.reset();
    }

    public boolean finished(){
        return atCross;
    }

    /**
     * Continue following the line.
     * @return true if has not reached cross at end. false if has reached end.
     */
    public boolean followLine() {
        double steeringGain; // the amount of steering correction to apply this time
        double speed, turn; //values to use on drive object

        // follow line unless robot has reached "T" at end or the power profile
        // has been used
        if ((time = timer.get()) < powerProfile.length && !atCross) {
            int timeInSeconds = (int) time;//time to use to get element of powerProfile

            // read the sensors
            int leftValue = left.get() ? 1 : 0;
            int middleValue = middle.get() ? 1 : 0;
            int rightValue = right.get() ? 1 : 0;

            // compute the single value from the 3 sensors. Notice that the bits
            // for the outside sensors are flipped depending on left or right
            // fork. Also the sign of the steering direction is different for left/right.
            if (goLeft) {
                binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
                steeringGain = -defaultSteeringGain;
            } else {
                binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
                steeringGain = defaultSteeringGain;
            }

            // get the default speed and turn rate at this time
            speed = powerProfile[timeInSeconds];
            turn = 0;

            // different cases for different line tracking sensor readings
            switch (binaryValue) {
                case 1:  // on line edge (outside)
                    turn = 0;
                    break;
                case 7:  // all sensors on (maybe at cross)
                    //ignore cross unless we are at the time we expect to see it
                    if (time > stopTime) {
                        atCross = true;
                        speed = 0;
                    }
                    break;
                case 0:  // all sensors off
                    // if on outside edge or has not seen line yet
                    if (previousValue == 0 || previousValue == 1) {
                        turn = steeringGain;
                    }
                    // otherwise on inside edge
                    else {
                        turn = -steeringGain;
                    }
                    break;
                default:  // all other cases
                    //turn out to the line edge
                    turn = -steeringGain;
            }
            // print current status for debugging
            if (binaryValue != previousValue) {
                System.out.println("Time: " + time + " Sensor: " + binaryValue + " speed: " + speed + " turn: " + turn + " atCross: " + atCross);
            }

            // set the robot speed and direction
            drive.arcadeDrive(speed, turn * speed);

            // store current line tracker state previous state as long as it is
            // not zero.  This way, if we go completely off the side of the line
            // previousValue will keep the state of the last time we saw the
            // line and we know which side of the line we went off on
            if (binaryValue != 0) {
                previousValue = binaryValue;
            }
            return true;
        }
        else{
            // Done with loop - stop the robot. Robot ought to be at the end of the line
            drive.arcadeDrive(0, 0);
            return false;
        }
    }

    public void setDefaultSteeringGain(double val){
        defaultSteeringGain = val;
    }
}
